/*
 * Copyright (c) 2016, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

//*****************************************************************************
//
//! \addtogroup motor_driver_control
//! @{
//
//*****************************************************************************

/* Standard includes                                                          */
#include <stdlib.h>
#include <unistd.h>

/* TI-Drivers Includes */
#include <ti/drivers/GPIO.h>
#include <ti/drivers/PWM.h>
#include <ti/display/Display.h>

/* Module Header */
#include <ti/sail/bmi160/bmi160.h>

/* Application includes                                                       */
#include "Board.h"
#include "wifi_doorlock_app.h"
#include "motor_driver_if.h"
#include "bmi160_support.h"

/****************************************************************************************************************
                   GLOBAL VARIABLES
****************************************************************************************************************/
/* Overall application control block */
extern Application_CB App_CB;
extern Display_Handle display;

//*****************************************************************************
//
//! Motor_SendMsgToQueue - Utility function that receive motorMsg parameter and
//! tries to push it the queue with minimal time for timeout of 0.
//! If the queue isn't full the parameter will be stored and the function
//! will return 0.
//! If the queue is full and the timeout expired (because the timeout parameter
//! is 0 it will expire immediately), the parameter is thrown away and the
//! function will return -1 as an error for full queue.
//!
//! \param[in] struct motorMsg *queueElement
//!
//! \return 0 on success, -1 on error
//
//*****************************************************************************
int32_t Motor_SendMsgToQueue(struct motorMsg *queueElement)
{
    struct timespec abstime = { 0 };

    clock_gettime(CLOCK_REALTIME, &abstime);

    if (App_CB.motorQueue)
    {
        /* send message to the queue                                         */
        if (mq_timedsend(App_CB.motorQueue, (char *) queueElement,
                         sizeof(struct motorMsg), 0, &abstime) == 0)
        {
            return 0;
        }
    }
    return -1;
}


/* Sets stopped state for motor control */
void Motor_setStoppedState(PWM_Handle *pwm)
{
    GPIO_write(Board_GPIO_nSLEEP, 0);
    GPIO_write(Board_GPIO_IN1, 0);
    PWM_setDuty(*pwm, 0);
}

/* Motor Driver Task */
void *MotorDriverControl(void *arg0)
{
    struct motorMsg msg;
    BMI160Event event;

    /* Period and duty in microseconds */
    uint32_t pwmPeriod = PWM_PERIOD_uSEC;    // 64 uS ==> 15625 Hz
    /* Sleep time in microseconds */
    PWM_Handle pwm1 = NULL;
    PWM_Params params;

    PWM_Params_init(&params);
    params.periodUnits = PWM_PERIOD_US;
    params.periodValue = pwmPeriod;
    params.dutyUnits = PWM_DUTY_US;
    params.dutyValue = 0;

    sem_post(&App_CB.motorReady);
    sem_wait(&App_CB.startBarrier);

    while (1)
    {
        /* waiting for signals                                                */
        mq_receive(App_CB.motorQueue, (char*) &msg, sizeof(struct motorMsg),
                   NULL);

        /* Init PWM for controlling motor driver */
        PWM_init();
        pwm1 = PWM_open(Board_PWM0, &params);
        if (pwm1 == NULL)
        {
            Display_printf(display, 0, 0,
                           "\t [Motor Thread] Error - Failed to open PWM0");
            /* Board_PWM0 did not open */
            while (1);
        }

#if (SENSORS == 1)
        /* Signal measure start to BMI thread */
        event = BMI160_START;
        mq_send(App_CB.bmi160Queue, (char*)&event, sizeof(BMI160Event), 0);
#endif

        /* Set default pin states for controlling motor driver */
        GPIO_write(Board_GPIO_nSLEEP, 0);
        GPIO_write(Board_GPIO_IN1, 0);
        PWM_start(pwm1);

        switch (msg.event)
        {
        case LOCK_MOTOR_CMD:

            Display_printf(display, 0, 0,
                           "[Motor Thread] Locking");
            /* Bring motor driver out of sleep and set direction */
            GPIO_write(Board_GPIO_IN1, 1);
            GPIO_write(Board_GPIO_nSLEEP, 1);

            /* Set duty for driving motor forward */
            PWM_setDuty(pwm1, PWM_DUTY_LOCK_uSEC);
            /* Sleep 2 seconds */
            sleep(2);

            /* Reset duty values, completely stop motor, put motor driver back to sleep */
            Motor_setStoppedState(&pwm1);
            break;

        case UNLOCK_MOTOR_CMD:

            Display_printf(display, 0, 0,
                                       "[Motor Thread] Unlocking");

            /* Bring motor driver out of sleep and set direction */
            GPIO_write(Board_GPIO_IN1, 0);
            GPIO_write(Board_GPIO_nSLEEP, 1);

            /* Set duty for driving motor backward */
            PWM_setDuty(pwm1, PWM_DUTY_UNLOCK_uSEC);
            /* Sleep 2 seconds */
            sleep(2);

            /* Reset duty values, completely stop motor, put motor driver back to sleep */
            Motor_setStoppedState(&pwm1);
            break;
        }

        PWM_stop(pwm1);
        PWM_close(pwm1);

#if (SENSORS == 1)
        /* Signal measure stop to BMI thread */
        event = BMI160_STOP;
        mq_send(App_CB.bmi160Queue, (char*)&event, sizeof(BMI160Event), 0);
#endif
    }
}
//*****************************************************************************
//
// Close the Doxygen group.
//! @}
//
//*****************************************************************************
